//
#include "PropInteDiff.h"
//
//*****************************************************************************
//!\brief	The speed PID initialization
//!\param	None
//!\retval  None                                                               
//*****************************************************************************
void Speed_PID_Init(LONG KP,LONG KI,LONG OUTMAX)
{
    SpeedPID.OutMax =  OUTMAX;
    SpeedPID.OutMin = -OUTMAX;
    SpeedPID.Kp = KP;
    SpeedPID.Ki = KI;
}

//*****************************************************************************
//!\brief	The speed PID reset
//!\param	None
//!\retval  None                                                               
//*****************************************************************************
void Speed_PID_Rest(void)
{
	SpeedPID.Err  = 0;
    SpeedPID.ErrBak  = 0;
	SpeedPID.Uout = 0;
    SpeedPID.UoutBak = 0;
}

//*****************************************************************************
//!\brief	The speed PID control logic
//!\param	the set speed and the actual speed
//!\retval  the offset value of the speed
//*****************************************************************************
LONG Speed_PID_Ctrl(LONG ref,LONG fdb)
{LONG  Data32_tmp1 = 0,Data32_tmp2 = 0;
//
    SpeedPID.Ref = ref;		// the set data
    SpeedPID.Fdb = fdb;		// the actual data
    //-----Error-------------
    SpeedPID.Err = SpeedPID.Ref - SpeedPID.Fdb;
    //
    Data32_tmp1 = (LONG)(SpeedPID.Kp * (SpeedPID.Err - SpeedPID.ErrBak));
    Data32_tmp1 /= 1000;
    Data32_tmp2 = (LONG)(SpeedPID.Ki * SpeedPID.Err);
    Data32_tmp2 /= 1000;
    //
    SpeedPID.Uout = Data32_tmp1 + Data32_tmp2;
    //
    if(SpeedPID.Uout > SpeedPID.OutMax)
    {
        SpeedPID.Uout = SpeedPID.OutMax;
    }
    else if(SpeedPID.Uout < SpeedPID.OutMin)
    {
        SpeedPID.Uout = SpeedPID.OutMin;
    }
    //
    SpeedPID.ErrBak = SpeedPID.Err;
    //
    return SpeedPID.Uout;
}

//*****************************************************************************
//!\brief	The IBus PID initialization
//!\param	None
//!\retval  None                                                               
//*****************************************************************************
void IBus_PID_Init(LONG KP,LONG KI,LONG OUTMAX)
{
    IBusPID.OutMax =  OUTMAX;
    IBusPID.OutMin = -OUTMAX;
    IBusPID.Kp = KP;
    IBusPID.Ki = KI;
}

//*****************************************************************************
//!\brief	The IBus PID reset
//!\param	None
//!\retval  None                                                               
//*****************************************************************************
void IBus_PID_Rest(void)
{
    IBusPID.Err  = 0;
    IBusPID.ErrBak  = 0;
    IBusPID.Uout = 0;
    IBusPID.UoutBak = 0;
}

//*****************************************************************************
//!\brief	The IBus PID control logic
//!\param	the set IBus and the actual IBus
//!\retval  the offset value of the Ibus                                      
//*****************************************************************************
LONG IBus_PID_Ctrl(LONG ref,LONG fdb)
{LONG  Data32_tmp1 = 0,Data32_tmp2 = 0;
    //
    IBusPID.Ref = ref;     // the set data
    IBusPID.Fdb = fdb;     // the actual data
    //-----Error-------------
    IBusPID.Err = IBusPID.Ref - IBusPID.Fdb;
    //
    Data32_tmp1 = (LONG)(IBusPID.Kp * (IBusPID.Err - IBusPID.ErrBak));
    Data32_tmp1 /= 1000;
    Data32_tmp2 = (LONG)(IBusPID.Ki * IBusPID.Err);
    Data32_tmp2 /= 1000;
    //
    IBusPID.Uout = Data32_tmp1 + Data32_tmp2;
    //
    if(IBusPID.Uout > IBusPID.OutMax)
    {
        IBusPID.Uout = IBusPID.OutMax;
    }
    else if(IBusPID.Uout < IBusPID.OutMin)
    {
        IBusPID.Uout = IBusPID.OutMin;
    }
    //
    IBusPID.ErrBak = IBusPID.Err;
    //
    return IBusPID.Uout;
}

//*****************************************************************************
//!\brief	The IBus PID initialization
//!\param	None
//!\retval  None                                                                   
//*****************************************************************************
void Curr_D_PID_Init(LONG KP,LONG KI,LONG OUTMAX)
{
    Curr_D_PID.OutMax =  OUTMAX;
    Curr_D_PID.OutMin = -OUTMAX;
    Curr_D_PID.Kp = KP;
    Curr_D_PID.Ki = KI;
}

//*****************************************************************************
//!\brief	The IBus PID reset
//!\param	None
//!\retval  None                                                                   
//*****************************************************************************
void Curr_D_PID_Rest(void)
{
    Curr_D_PID.Err  = 0;
    Curr_D_PID.ErrBak  = 0;
    Curr_D_PID.Uout = 0;
    Curr_D_PID.UoutBak = 0;
}

//*****************************************************************************
//!\brief	The IBus PID control logic
//!\param	the set IBus and the actual IBus
//!\retval  the offset value of the Ibus                                                                   
//*****************************************************************************
LONG Curr_D_PID_Ctrl(LONG ref,LONG fdb)
{LONG  Data32_tmp1 = 0,Data32_tmp2 = 0;
    //
    Curr_D_PID.Ref = ref;     // the set data
    Curr_D_PID.Fdb = fdb;     // the actual data
    //-----Error-------------
    Curr_D_PID.Err = Curr_D_PID.Ref - Curr_D_PID.Fdb;
    //
    Data32_tmp1 = (LONG)(Curr_D_PID.Kp * (Curr_D_PID.Err - Curr_D_PID.ErrBak));
    Data32_tmp1 /= 1000;
    Data32_tmp2 = (LONG)(Curr_D_PID.Ki * Curr_D_PID.Err);
    Data32_tmp2 /= 1000;
    //
    Curr_D_PID.Uout = Data32_tmp1 + Data32_tmp2;
    //
    if(Curr_D_PID.Uout > Curr_D_PID.OutMax)
    {
        Curr_D_PID.Uout = Curr_D_PID.OutMax;
    }
    else if(Curr_D_PID.Uout < Curr_D_PID.OutMin)
    {
        Curr_D_PID.Uout = Curr_D_PID.OutMin;
    }
    //
    Curr_D_PID.ErrBak = Curr_D_PID.Err;
    //
    return Curr_D_PID.Uout;
}

//*****************************************************************************
//!\brief	The IBus PID initialization
//!\param	None
//!\retval  None                                                                   
//*****************************************************************************
void Curr_Q_PID_Init(LONG KP,LONG KI,LONG OUTMAX)
{
    Curr_Q_PID.OutMax =  OUTMAX; 
    Curr_Q_PID.OutMin = -OUTMAX;
    Curr_Q_PID.Kp = KP;
    Curr_Q_PID.Ki = KI; 
}

//*****************************************************************************
//!\brief	The IBus PID reset
//!\param	None
//!\retval  None                                                                   
//*****************************************************************************
void Curr_Q_PID_Rest(void)
{
    Curr_Q_PID.Err  = 0;
    Curr_Q_PID.ErrBak  = 0;
    Curr_Q_PID.Uout = 0;
    Curr_Q_PID.UoutBak = 0;
}

//*****************************************************************************
//!\brief	The IBus PID control logic
//!\param	the set IBus and the actual IBus
//!\retval  the offset value of the Ibus                                                                   
//*****************************************************************************
LONG Curr_Q_PID_Ctrl(LONG ref,LONG fdb)
{LONG  Data32_tmp1 = 0,Data32_tmp2 = 0;
    //
    Curr_Q_PID.Ref = ref;     // the set data
    Curr_Q_PID.Fdb = fdb;     // the actual data
    //-----Error-------------
    Curr_Q_PID.Err = Curr_Q_PID.Ref - Curr_Q_PID.Fdb;
    //
    Data32_tmp1 = (LONG)(Curr_Q_PID.Kp * (Curr_Q_PID.Err - Curr_Q_PID.ErrBak));
    Data32_tmp1 /= 1000;
    Data32_tmp2 = (LONG)(Curr_Q_PID.Ki * Curr_Q_PID.Err);
    Data32_tmp2 /= 1000;
    //
    Curr_Q_PID.Uout = Data32_tmp1 + Data32_tmp2;
    //
    if(Curr_Q_PID.Uout > Curr_Q_PID.OutMax)
    {
        Curr_Q_PID.Uout = Curr_Q_PID.OutMax;
    }
    else if(Curr_Q_PID.Uout < Curr_Q_PID.OutMin)
    {
        Curr_Q_PID.Uout = Curr_Q_PID.OutMin;
    }
    //
    Curr_Q_PID.ErrBak = Curr_Q_PID.Err;
    //
    return Curr_Q_PID.Uout;
}

//=======================================================================================================

